# 导入sensor, image, time, math四个模块
import sensor, image, time, math
from pyb import UART

sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)          # 设置摄像头的像素格式为灰度
sensor.set_framesize(sensor.QVGA)       # 设置摄像头的分辨率为QVGA (320x240)
sensor.skip_frames(time = 2000)     # 跳过前2000帧，等待摄像头稳定
sensor.set_auto_gain(False)     # 关闭自动增益，否则会影响颜色跟踪
sensor.set_auto_whitebal(False)     # 关闭自动白平衡，否则会影响颜色跟踪
clock = time.clock()# 创建一个时钟对象，用于计算帧率

uart = UART(3, 115200)

Binary_thresholds = (0, 72)     # 定义二值化的阈值，(a, b)表示灰度值在a到b之间的像素会被设置为255（白色），其余的像素会被设置为0（黑色）
find_thresholds =(0,50)     # 定义寻找色块的阈值，(0,50)表示灰度值在0到50之间的像素会被认为是同一个色块

# 初始化三个变量，分别表示左边连通域的中心x坐标，右边连通域的中心x坐标，和两个连通域中心的平均x坐标
x1 = 0 #左边坐标
x2 = 0 #右边的坐标
mid_zb = 0 #中点坐标

# 定义两个矩形区域，分别用于在图像上划分左右两个感兴趣区域（ROI）
r1 =(0,130,100,25) # 左边的矩形区域，(x,y,w,h)表示左上角坐标为(0,130)，宽度为100，高度为25
r2 =(180,130,180,25) # 右边的矩形区域，(x,y,w,h)表示左上角坐标为(180,130)，宽度为180，高度为25

#判断是否左右线都有识别到
left_flag = 0
right_flag = 0

# 主循环
while(True):
    # 更新时钟
    clock.tick()
    # 拍摄一张图像并存储在img变量中
    img = sensor.snapshot()
    # 对图像进行二值化处理，把灰度值在阈值范围内的像素变成白色，其余的变成黑色，并且反转颜色（invert=True）
    img.binary([Binary_thresholds],invert=True)
    # 在图像上画出两个矩形区域，颜色为红色（255,0,0）
    img.draw_rectangle(r1,color=(255,0,0))
    img.draw_rectangle(r2,color=(255,0,0))
    # 在左边的矩形区域内寻找连通域，要求像素阈值为200，面积阈值为200，并且合并重叠的连通域（merge=True）
    for blob in img.find_blobs([find_thresholds], roi = r1,pixels_threshold=200, area_threshold=200, merge=True):
        # 在图像上画出找到的连通域的外接矩形，颜色为红色（255,0,0）
        img.draw_rectangle(blob.rect(),color=(255,0,0))
        # 更新左边连通域的中心x坐标
        left_flag = 1
        x1 = blob.cx()
        # 在图像上画出找到的连通域的中心点，颜色为红色（255,0,0）
        img.draw_cross(blob.cx(), blob.cy(),color=(255,0,0))
        # 打印左边连通域的中心x坐标
        #print(x1)
    # 在右边的矩形区域内寻找连通域，要求像素阈值为200，面积阈值为200，并且合并重叠的连通域（merge=True）
    for blob in img.find_blobs([find_thresholds], roi = r2,pixels_threshold=200, area_threshold=200, merge=True):
        # 在图像上画出找到的连通域的外接矩形，颜色为红色（255,0,0）
        img.draw_rectangle(blob.rect(),color=(255,0,0))
        right_flag = 1
        # 更新右边连通域的中心x坐标
        x2 = blob.cx()
        # 在图像上画出找到的连通域的中心点，颜色为红色（255,0,0）
        img.draw_cross(blob.cx(), blob.cy(),color=(255,0,0))
        #print(x2)# 打印右边连通域的中心x坐标

    mid_zb = (x1+x2)/2      # 计算两个连通域中心的平均x坐标
    #print(mid_zb)       # 打印中心坐标

    error = mid_zb - 160 #获取与中点误差
    if left_flag == 1 and right_flag == 1:
        output_str="GND=%d#" % (error)
        uart.write(output_str)
    else:
        error = 0
        output_str="GND=%d#" % (error)
        uart.write(output_str)
    left_flag = 0
    right_flag = 0
    print(error)
